clear;clc;

load photos;
pas = 15;
margin = 9;
pasRad = (pas / 180) * pi;
world = depthTo2DCloud(fliplr(images{1}), margin);

% subplot(1,3,1);
plot(world(:,1), world(:,2), 'r.'); axis equal;
pause(0.1);

pose = [0 0 0];
poseVect = pose;
for i = [1 : ((360/pas) - 1)]
% for i = [1:5]
    pose(3) = pose(3) + pasRad ;
    pose(1) = 0;
    pose(2) = 0;
    newpoints = depthTo2DCloud(fliplr(images{i + 1}), margin);
    [world,pose] = getCurrentLocationAndUpdate(world(:,1:2),newpoints, pose, 0.74);
    poseVect = [poseVect;pose];
    i
    plot(world(:,1), world(:,2), 'r.'); axis equal;
    pause(0.1);
end
% subplot(1,3,3);
plot(world(:,1), world(:,2), 'r.'); axis equal;